Plane: support off-board terrain navigation #26262

srmainwaring · 2024-02-19T17:08:43Z

This is a proof-of-concept implementation of off-board control for ArduPlane for use with the terrain navigation library.

The implementation uses the SET_POSITION_TARGET_GLOBAL_INT mavlink message to match the interface provided by terrain_navigation and adds a custom guided mode (TERRAIN_NAVIGATION = Mode(26)) to minimise the changes in current code. The longer term objective is to use the DDS interface and integrate more closely with the existing controller and guided mode.

There are two choices of controller at compile time. Selection is by the variable in mode_terrain_navigation.cpp:

#define ENABLE_NPFG_CONTROLLER 0

Setting ENABLE_NPFG_CONTROLLER to 1 selects an experimental integration of the Nonlinear Path Following Guidance (NPFG) controller. Unfortunately this is not yet working correctly. The other approach co-opts the existing L1 controller to follow the Dubins path provided by the external controller.

Interpreting setpoint information

The planner populates the fields of SET_POSITION_TARGET_GLOBAL_INT as follows:

The position (lat_int, lon_int, alt) is the closest point on the planned Dubins path to the current vehicle location, where the vehicle location is provided to the planner by the flight controller using the GLOBAL_POSITION_INT message. The alt is always provided as MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, so this is the only case handled.

The horizontal velocity components (vx, vy) comprise a unit vector tangential to the Dubins path at the closest point. The vertical component (vz) is the desired climb rate along the flightpath.

The horizontal acceleration components determine the desired path curvature. The path radius and direction are calculated from the lateral acceleration using r = |v|^2 / |a_lat|.

If the acceleration is zero, the curvature is assumed to be zero (a radius of zero is interpreted as zero curvature).

terrain navigation mode

When using the L1 controller, the terrain navigation mode uses update_loiter if the curvature is non-zero (radius non-zero) and update_waypoint if the curvature is zero (radius zero). When using update_loiter the setpoint position, tangent and turn radius are used to set a center_wp, turn radius and direction. When using update_waypoint the setpoint position and tangent are used to set a next_wp 50m ahead of the setpoint position.

Usage

In the following we assume the colcon workspace for the ROS 2 packages is called ros2-aerial.

Build dependencies

Follow the instructions to set up ArduPilot for ROS 2 Humble.

Build and install the ros2 branch of terrain navigation and any dependencies.

Run

Launch ArduPilot SITL. This launch file simulates a quadplane in a park in Davosdorf.

ros2 launch terrain_navigation_ros ardupilot.launch.py

Connect a mavproxy session to the running simulation:

mavproxy.py --master 127.0.0.1:14550 --console --map

Open the mission editor and load the misson:

ros2-aerial/src/terrain_navigation/terrain_navigation_ros/config/davosdorf_mission.txt

Update a couple of parameters

param set SCALING_SPEED 25.0 # to match AIRSPEED_CRUISE
param set WP_LOITER_RAD 80.0

Run the mission

FBWA> arm throttle
FBWA> auto

The aircraft will takeoff then loiter indefinitely.

Launch mavros configured for ardupilot:

ros2 launch terrain_navigation_ros mavros.launch.py flight_stack:=ardupilot

Launch the terrain planner:

ros2 launch terrain_navigation_ros terrain_planner.launch.py rviz:=false cruise_speed:=25.0 minimum_turn_radius:=80

Launch rviz:

ros2 launch terrain_navigation_ros rviz.launch.py

Click the Load Terrain button.
Select Interact in the top tool bar.
Move the interactive marker over where the plane is loitering and click Update Start.
Move the interactive marker to another location and click Update Goal.
A green circle will appear if the goal location is valid. A red circle indicates the goal location is not suitable.
Click Plan.
Click Engage Planner.
Click NAVIGATE.

In MAVProxy you should see the mode has switched to the custom mode (Mode(26) = TERRAIN_NAVIGATION)

AUTO>
Mode(26)>

Testing

NPFG

Is not working as expected and is disabled.

L1

Is satisfactory, although the turns are not tight (the vehicle rate of turn is less than desired).

Figure: The vehicle turn is wider than the setpoint. The red arrow is the desired turn radius and direction. The blue arrow is the path tangent.

Figure: The final loiter circle is wider than the setpoint.

Figure: Logged flight path

IamPete1 · 2024-02-25T13:38:09Z

I guess your radius's are turning out wrong because of the altitude scale we apply for constant load factor.

ardupilot/libraries/AP_L1_Control/AP_L1_Control.cpp

Lines 148 to 178 in 2a67cbe

    
           float AP_L1_Control::loiter_radius(const float radius) const 
        
           { 
        
               // prevent an insane loiter bank limit 
        
               float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f); 
        
               float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS; 
        
               float nominal_velocity_sea_level = 0.0f; 
        
               if(_tecs != nullptr) { 
        
                   nominal_velocity_sea_level =  _tecs->get_target_airspeed(); 
        
               } 
        
               float eas2tas_sq = sq(_ahrs.get_EAS2TAS()); 
        
               if (is_zero(sanitized_bank_limit) || is_zero(nominal_velocity_sea_level) || 
        
                   is_zero(lateral_accel_sea_level)) { 
        
                   // Missing a sane input for calculating the limit, or the user has 
        
                   // requested a straight scaling with altitude. This will always vary 
        
                   // with the current altitude, but will at least protect the airframe 
        
                   return radius * eas2tas_sq; 
        
               } else { 
        
                   float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level; 
        
                   if (sea_level_radius > radius) { 
        
                       // If we've told the plane that its sea level radius is unachievable fallback to 
        
                       // straight altitude scaling 
        
                       return radius * eas2tas_sq; 
        
                   } else { 
        
                       // select the requested radius, or the required altitude scale, whichever is safer 
        
                       return MAX(sea_level_radius * eas2tas_sq, radius); 
        
                   } 
        
               } 
        
           }

There has been some discussion on this recently. I think the general view is that we should remove it (by default anyway) because it makes mission planning very unintuitive.

srmainwaring · 2024-02-25T13:41:03Z

I guess your radius's are turning out wrong because of the altitude scale we apply for constant load factor.

Thanks @IamPete1, I'll try disabling.

Update

Confirmed - the scaling in

float AP_L1_Control::loiter_radius(const float radius) const

is the cause.

- Extend to handle position as well as altitude. - Check for non-zero velocity and acceleration - Calculate and update loiter radius and direction. - Force update of adjust_altitude_target. - Disable debug output to GCS. Signed-off-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

- Initial implementation forwards to GUIDED. - Add mode to parameters. - Send msg to GCS when switching to TERRAIN_NAVIGATION Signed-off-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

- Copy npfg implementation from PX4 - Port npfg to ArduPilot - Replace tabs in npfg with whitespace - Change suffix in npfg header - Add NPFG instance to AP_Navigation adapter - Add parameters - Define static const variables in cpp - Update param group name Signed-off-by: Rhys Mainwaring <[email protected]>

- Add NPFG parameters - Add NPFG controller - Partially implement terrain navigation mode - Update set_position_target_global_int to use terrain navigation Signed-off-by: Rhys Mainwaring <[email protected]>

- Make getRollSetpoint const - Add method to set path tangent and implement some getters Signed-off-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

- Enable roll slew rate - Disable gain scaling to avoid div zero error - Correct control interval calc - Set default track keeping to enable - Update slew rate to 90 deg / s Signed-off-by: Rhys Mainwaring <[email protected]>

- Ensure radius is reset in terrain navigation mode - Co-opt L1 nav controller for path following in terrain navigation mode - Correct position of loiter centre - Update curvature calculation - Correct tangent vector Signed-off-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

- Implement method update_path - Update comment Signed-off-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

- Call ModeGuided _enter and _exit methods. - Use ModeGuided update method. - Store commanded curvature rather than radius. Signed-off-by: Rhys Mainwaring <[email protected]>

Signed-off-by: Rhys Mainwaring <[email protected]>

srmainwaring marked this pull request as draft February 19, 2024 17:08

srmainwaring added the ROS label Feb 19, 2024

srmainwaring mentioned this pull request Feb 19, 2024

Plane: extend support of SET_POSITION_TARGET_GLOBAL_INT #25754

Closed

3 tasks

srmainwaring force-pushed the prs/pr-plane-npfg branch 5 times, most recently from 0e46f35 to 23efb73 Compare February 25, 2024 11:23

srmainwaring added 13 commits February 26, 2024 10:26

RC_Channel: add new mode TERRAIN_NAVIGATION

03c4934

Signed-off-by: Rhys Mainwaring <[email protected]>

Plane: add new mode TERRAIN_NAVIGATION

d3ef7db

- Initial implementation forwards to GUIDED. - Add mode to parameters. - Send msg to GCS when switching to TERRAIN_NAVIGATION Signed-off-by: Rhys Mainwaring <[email protected]>

AP_NPFG: add library for non-linear path following guidance

2bffc45

Signed-off-by: Rhys Mainwaring <[email protected]>

Plane: include AP_NPFG library

f034e0d

Signed-off-by: Rhys Mainwaring <[email protected]>

Plane: add terrain navigation mode

946db97

- Add NPFG parameters - Add NPFG controller - Partially implement terrain navigation mode - Update set_position_target_global_int to use terrain navigation Signed-off-by: Rhys Mainwaring <[email protected]>

AP_NPFG: add method to set path tangent

ce81b91

- Make getRollSetpoint const - Add method to set path tangent and implement some getters Signed-off-by: Rhys Mainwaring <[email protected]>

Plane: set desired path tangent in terrain navigation mode

b4f0c17

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_NPFG: move airspeed updates to update_loiter

d89f59e

Signed-off-by: Rhys Mainwaring <[email protected]>

Plane: set radius and path tangent before handle guided request

88405cc

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_NPFG: set control interval and enable roll slew rate

01db379

- Enable roll slew rate - Disable gain scaling to avoid div zero error - Correct control interval calc - Set default track keeping to enable - Update slew rate to 90 deg / s Signed-off-by: Rhys Mainwaring <[email protected]>

srmainwaring force-pushed the prs/pr-plane-npfg branch from 23efb73 to 80956f6 Compare February 26, 2024 10:36

srmainwaring added 5 commits February 26, 2024 10:51

Plane: check SET_POSITION_TARGET_GLOBAL_INT bit mask for path following

6bc8930

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_Navigation: add method update_path

0e7f307

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_L1_Control: add method update_path

348185a

- Implement method update_path - Update comment Signed-off-by: Rhys Mainwaring <[email protected]>

AP_NPFG: add method update_path

10fe4bf

Signed-off-by: Rhys Mainwaring <[email protected]>

Plane: terrain_navigation: use method nav_controller->update_path

70ae8da

- Call ModeGuided _enter and _exit methods. - Use ModeGuided update method. - Store commanded curvature rather than radius. Signed-off-by: Rhys Mainwaring <[email protected]>

srmainwaring force-pushed the prs/pr-plane-npfg branch from 482770a to 70ae8da Compare February 26, 2024 12:54

AP_NPFG: move update code from update_loiter to update_path

427f6cd

Signed-off-by: Rhys Mainwaring <[email protected]>

srmainwaring mentioned this pull request Feb 26, 2024

Plane: handle guided path requests in GUIDED mode #26328

Open

4 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Plane: support off-board terrain navigation #26262

Plane: support off-board terrain navigation #26262

srmainwaring commented Feb 19, 2024 •

edited

Loading

IamPete1 commented Feb 25, 2024

srmainwaring commented Feb 25, 2024 •

edited

Loading

Plane: support off-board terrain navigation #26262

Are you sure you want to change the base?

Plane: support off-board terrain navigation #26262

Conversation

srmainwaring commented Feb 19, 2024 • edited Loading

Interpreting setpoint information

terrain navigation mode

See also

Usage

Build dependencies

Run

Testing

NPFG

L1

IamPete1 commented Feb 25, 2024

srmainwaring commented Feb 25, 2024 • edited Loading

srmainwaring commented Feb 19, 2024 •

edited

Loading

srmainwaring commented Feb 25, 2024 •

edited

Loading